import time
import math
import pydobot
from serial.tools import list_ports

def linear_traj(x0,y0,z0,x1,y1,z1,t,step):
    x = x0*(1-t/step) + x1*t/step
    y = y0*(1-t/step) + y1*t/step
    z = z0*(1-t/step) + z1*t/step
    return [x, y, z]

def quintic_traj(x0,y0,z0,x1,y1,z1,t,step):
    A = list[x0, y0, z0]
    B = list[x1, y1, z1]
    x = A + (B - A) * (10 * math.pow((t/step),3) - 15 * math.pow((t/step),4) + 6 * math.pow((t/step), 5)
    return [x[0], x[1], x[2]]

if __name__ == '__main__':
    available_ports = list_ports.comports()
    print(f'available ports: {[x.device for x in available_ports]}')
    port = available_ports[0].device
    device = pydobot.Dobot(port=port, verbose=True)
    # 获取关节角度
    (x, y, z, r, j1, j2, j3, j4) = device.pose()
    print(f'x:{x} y:{y} z:{z} j1:{j1} j2:{j2} j3:{j3} j4:{j4}')
    # 到初始位置
    x0=72.5; y0=5.7; z0=-11;
    device.move_to(x0, y0, z0, r, wait=True)
    # time.sleep(1000)

    x1=103.8; y1=20.6; z1=-30.0228;
    # 开吸盘
    device.suck(enable=True)
    step = 100
    t = 1
    # 移动到指定位置
    for t in range(1, step):
        xm,ym,zm=linear_traj(x0,y0,z0,x1,y1,z1,t,step)
        device.move_to(xm, ym, zm, r, wait=True)

    # 放吸盘
    device.suck(enable=False)
    (xe,ye,ze,re,j1, j2, j3, j4)=device.pose()
    print("pose end", "Xe", xe, "Ye",  ye, "Ze", ze)
    # 关闭设备
    device.close()
